#include "motor.h"
#include "hmi_connect.h"
#include "stdbool.h"

UART_HandleTypeDef STM32_USARTHandle={0};
uint8_t stm32reciveflag;
uint8_t STM32_RX_BUF[64];
uint8_t STM32_RX_CNT;

extern uint16_t autoCarVelocity,autoLineWidth;
extern uint16_t motorCurrentPos;//电机实时位置
extern uint16_t motorCorrectPos;
//串口通讯 STM32H730
uint8_t motorDataFrame[15];
uint16_t sMotorVelPos,sMotorAbsPos,sMotorRunMode;
uint8_t sMotor01Dir,sMotor02Dir,sMotor03Dir,sMotor04Dir,CorrectVar;
uint8_t reZero;
extern uint16_t manulCarVelocity;//

motor_pos_t stuMotorPos = {.pos_updataflag = false};//读取电机当前位置
uint8_t readServoPos[8];//电机32位值串口通讯返回

extern uint16_t manualRun,autoRun;//手自动 
extern float correctfactor;
extern uint32_t CorrectVel; //修正速度

/*********
* @brief 电机位置-速度-前馈力矩控制，单位是0.00001圈
* @brief 电机速度-前馈力矩控制，单位是0.00001圈
*
*********/
void motor_slave(uint16_t carDir,uint8_t commond,uint8_t correctVal)
{  
    //carDir  左侧轮子正转 反转 右侧轮子 正转反转 
    //发送轮子速度 左侧轮 右侧轮  
    //手动运行
    if(autoRun==0){
        switch (carDir)
        {
            case 1:
                motor_send(manulCarVelocity,manulCarVelocity,motorCurrentPos,commond,1,2,1,2,correctVal,reZero);//前
            break;
            case 2:
                motor_send(manulCarVelocity,manulCarVelocity,motorCurrentPos,commond,2,1,2,1,correctVal,reZero);//后
            break;
            case 3:
                motor_send((manulCarVelocity-(CorrectVar*2)),(manulCarVelocity+(CorrectVar*2)),motorCurrentPos,commond,2,2,2,2,correctVal,reZero);//左转
            break;
            case 4:
                motor_send((manulCarVelocity+(CorrectVar*2)),(manulCarVelocity-(CorrectVar*2)),motorCurrentPos,commond,1,1,1,1,correctVal,reZero);//右转
            break;
            case 5:
                motor_send(manulCarVelocity,manulCarVelocity,motorCurrentPos,commond,1,2,1,2,correctVal,reZero);//左移动
            break;
            case 6:
                motor_send(manulCarVelocity,manulCarVelocity,motorCurrentPos,commond,2,1,2,1,correctVal,reZero);//右移动
            break;
            case 0:
                motor_send(0,0,0,commond,1,2,1,2,correctVal,reZero);
            break;
            default:
            break;
        }  
    //自动运行
    }else if(autoRun==1){
        switch (carDir)
        {
            case 1:
                motor_send(autoCarVelocity,autoCarVelocity,motorCurrentPos,commond,1,2,1,2,correctVal,reZero);//前
            break;
            case 2:
                motor_send(autoCarVelocity,autoCarVelocity,motorCurrentPos,commond,2,1,2,1,correctVal,reZero);//后
            break;
            case 3:
                motor_send((autoCarVelocity-(CorrectVar*2)),(autoCarVelocity+(CorrectVar*2)),motorCurrentPos,commond,2,2,2,2,correctVal,reZero);//左转
            break;
            case 4:
                motor_send((autoCarVelocity+(CorrectVar*2)),(autoCarVelocity-(CorrectVar*2)),motorCurrentPos,commond,1,1,1,1,correctVal,reZero);//右转
            break;
            case 5:
                motor_send(autoCarVelocity,autoCarVelocity,motorCurrentPos,commond,1,2,1,2,correctVal,reZero);//左移动
            break;
            case 6:
                motor_send(autoCarVelocity,autoCarVelocity,motorCurrentPos,commond,2,1,2,1,correctVal,reZero);//右移动
            break;
            case 7:
                motor_send((CorrectVel + (correctfactor * CorrectVel)),(CorrectVel - (correctfactor * CorrectVel)),
                            motorCorrectPos,commond,2,2,2,2,correctVal,reZero);//右修正
            break;
            case 8:
                motor_send((CorrectVel - (correctfactor * CorrectVel)),(CorrectVel + (correctfactor * CorrectVel)),
							motorCorrectPos,commond,1,1,1,1,correctVal,reZero);//左修正
            break;
                // motor_send((autoCarVelocity+(CorrectVar*2)),(autoCarVelocity-(CorrectVar*2)),motorCurrentPos,commond,1,1,1,1,correctVal,reZero);//右转
            case 9:
                motor_send(autoCarVelocity,(autoCarVelocity - (autoCarVelocity * 0.05)),motorCurrentPos,commond,1,2,1,2,correctVal,reZero);//前
            break;
            case 10:
                motor_send((autoCarVelocity - (autoCarVelocity * 0.05)),autoCarVelocity,motorCurrentPos,commond,1,2,1,2,correctVal,reZero);//前
            break;
            case 11:
                motor_send(autoCarVelocity,(autoCarVelocity - (autoCarVelocity * 0.05)),motorCurrentPos,commond,2,1,2,1,correctVal,reZero);//后
            break;
            case 12:
                motor_send((autoCarVelocity - (autoCarVelocity * 0.05)),autoCarVelocity,motorCurrentPos,commond,2,1,2,1,correctVal,reZero);//后
            break;
            case 0:
                motor_send(0,0,0,commond,1,2,1,2,correctVal,reZero);
            break;
            default:
            break;
        }
    }
    
    
    
}

void motor_connect_init(uint16_t bound)
{
     /*开启时钟*/
     __HAL_RCC_GPIOB_CLK_ENABLE();  
     __HAL_RCC_GPIOD_CLK_ENABLE();    
     __HAL_RCC_UART5_CLK_ENABLE();

    GPIO_InitTypeDef GPIO_InitStructure={0};
    
    //TX
    GPIO_InitStructure.Pin=GPIO_PIN_13;
    GPIO_InitStructure.Mode=GPIO_MODE_AF_PP;
    GPIO_InitStructure.Pull=GPIO_PULLUP;
    GPIO_InitStructure.Speed=GPIO_SPEED_FREQ_VERY_HIGH;
    GPIO_InitStructure.Alternate=GPIO_AF14_UART5;
    
    HAL_GPIO_Init(GPIOB,&GPIO_InitStructure);
    
    //RX
    GPIO_InitStructure.Pin=GPIO_PIN_2;
    GPIO_InitStructure.Mode=GPIO_MODE_AF_PP;
    GPIO_InitStructure.Pull=GPIO_PULLUP;
    GPIO_InitStructure.Speed=GPIO_SPEED_FREQ_VERY_HIGH;
    GPIO_InitStructure.Alternate=GPIO_AF8_UART5;
    
    HAL_GPIO_Init(GPIOD,&GPIO_InitStructure);



    STM32_USARTHandle.Instance=UART5;//UART5;			        //UART4
	STM32_USARTHandle.Init.BaudRate=bound;		        //波特率
	STM32_USARTHandle.Init.WordLength=UART_WORDLENGTH_8B;	//字长为8位数据格式
	STM32_USARTHandle.Init.StopBits=UART_STOPBITS_1;		//一个停止位
	STM32_USARTHandle.Init.Parity=UART_PARITY_NONE;		//无奇偶校验位
	STM32_USARTHandle.Init.HwFlowCtl=UART_HWCONTROL_NONE;	//无硬件流控
	STM32_USARTHandle.Init.Mode=UART_MODE_TX_RX;		    //收发模式
    
//    STM32_USARTHandle.Init.OverSampling    = UART_OVERSAMPLING_16;            // 过采样
//    STM32_USARTHandle.Init.OneBitSampling  = UART_ONE_BIT_SAMPLE_DISABLE;     // 采样位
//    STM32_USARTHandle.Init.Prescaler=UART_PRESCALER_DIV1;
//    STM32_USARTHandle.Init.TXFIFOThreshold = UART_TXFIFO_THRESHOLD_1_8;       // 发送FIFO阈值
//    STM32_USARTHandle.Init.RXFIFOThreshold = UART_RXFIFO_THRESHOLD_1_8;       // 接收FIFO阈值 
//    STM32_USARTHandle.AdvancedInit.AdvFeatureInit =UART_ADVFEATURE_NO_INIT;
//    
    
	HAL_UART_Init(&STM32_USARTHandle);			        //HAL_UART_Init()会使能UART
    
    __HAL_UART_CLEAR_IT(&STM32_USARTHandle,UART_CLEAR_TCF);//
    
   // #if STM32_USART_EN
    __HAL_UART_ENABLE_IT(&STM32_USARTHandle,UART_IT_RXNE);//开启接收中断
    HAL_NVIC_EnableIRQ(UART5_IRQn);				        //使能USART6中断
	HAL_NVIC_SetPriority(UART5_IRQn,4,0);			        //抢占优先级0，子优先级5
  
  // #endif
   
}



//*@prameter 左侧电机速度 右侧电机速度 电机位置 电机控制方式 电机1方向 电机2方向 电机3方向 电机4方向 电机差速修正系数 零点使能

void motor_send(uint16_t velL,uint16_t velR,int16_t pos ,uint8_t command,uint8_t m1dir,
    uint8_t m2dir,uint8_t m3dir,uint8_t m4dir,uint8_t correctVal,uint8_t zeroTaget){

   //写modbus地址第一个字节 uint6_t 
   uint16_t crcAss;  
       
   motorDataFrame[0]=0XAA;//
   motorDataFrame[1]=0XBB;// 
   //*(uint16_t *)&motorDataFrame[2]=vel;  
   motorDataFrame[2]=(uint8_t)(velL>>8)&0XFF;//CRC高位
   motorDataFrame[3]=(uint8_t)velL&0XFF;//CRC低位
   motorDataFrame[4]=(uint8_t)(velR>>8)&0XFF;//CRC高位
   motorDataFrame[5]=(uint8_t)velR&0XFF;//CRC低位  
        
   motorDataFrame[6]=(uint8_t)(pos>>8)&0XFF;//CRC高位
   motorDataFrame[7]=(uint8_t)pos&0XFF;//CRC低位        
        
   //*(uint16_t *)&motorDataFrame[4]=pos; 


        
   motorDataFrame[8]=command;     
   motorDataFrame[9]=m1dir; 
   motorDataFrame[10]=m2dir; 
   motorDataFrame[11]=correctVal; 
   motorDataFrame[12]=reZero;
        
   crcAss=(_bMBCRC16(motorDataFrame,13));//modbus校验
  // *(uint16_t *)&motorDataFrame[12]=crcAss;
    
   motorDataFrame[13]=(uint8_t)((crcAss>>8)&0XFF); 
   motorDataFrame[14]=(uint8_t)(crcAss&0XFF);     
   STM32_Send_Data(motorDataFrame,15);//串口数据发送

   HAL_Delay(100);//
//   
//   //*接收=======================================START
//    STM32_Receive_Data(readServoPos,&readL);
//    crcAss=(_bMBCRC16(readServoPos,6));//modbus校验
//    crcH=(uint8_t)((crcAss>>8)&0XFF); //CRC高位
//    crcL=(uint8_t)(crcAss&0XFF); //CRC低位
//    
//    
//   if((readServoPos[0]==0XAA)&&(readServoPos[1]==0XBB)&&(crcL==readServoPos[7])&&(crcH==readServoPos[6])){
////       for(uint8_t cc=0;cc<8;cc++)
////       {
////        readServoPos[cc];
////       
////       }
//       
//       
//       motorReadPosC=(readServoPos[5]<<24)|(readServoPos[4]<<16)|(readServoPos[3]<<8)|(readServoPos[2]); //数据移位  
//   }
//   HAL_Delay(70);//
//   //*接收=========================================END     
}

/**
  * @brief  向灯带控制器发送数据
  * @param  buf:接收缓存首地址 len:读到的数据长度
  *                
  * @retval 无
*/
void STM32_Send_Data(uint8_t *buf,uint8_t len)//发送数据给ROS
{
	
    HAL_UART_Transmit(&STM32_USARTHandle,buf,len,1000);//串口6发送数据
	STM32_RX_CNT=0;	  	

}

/**
  * @brief  STM32查询接收到的数据
  * @param  buf:接收缓存首地址 len:读到的数据长度 
  *                
  * @retval 无
*/
#if STM32_USART_EN   		//如果使能了接收   	 
void STM32_Receive_Data(uint8_t *buf,uint8_t *len)//接收ROS数据
{
	uint8_t rxlen=STM32_RX_CNT;
	uint8_t i=0;
	*len=0;				//默认为0
	HAL_Delay(10);		//等待10ms,连续超过10ms没有接收到一个数据,则认为接收结束
	if(rxlen==STM32_RX_CNT&&rxlen)//接收到了数据,且接收完成了
	{
		for(i=0;i<rxlen;i++)
		{
			buf[i]=STM32_RX_BUF[i];	
		}		
		*len=STM32_RX_CNT;	//记录本次数据长度
		STM32_RX_CNT=0;		//清零
	}
}
#endif

#if STM32_USART_EN   		//如果使能了接收   	  


/**
  * @brief  串口5接收中断
  * @param  null 
  *                
  * @retval null
*/
uint32_t uat5C;
void UART5_IRQHandler(void)
{
    uat5C++;
    uint8_t res;	  
    if(__HAL_UART_GET_IT(&STM32_USARTHandle,UART_IT_RXNE)!=RESET)  //接收中断
	{	 	
        HAL_UART_Receive(&STM32_USARTHandle,&res,1,1000);
        stm32reciveflag=1;
		if(STM32_RX_CNT<8)
		{
			STM32_RX_BUF[STM32_RX_CNT]=res;		//记录接收到的值
			STM32_RX_CNT++;						//接收数据增加1 
		} 
	} 
}    
#endif


/**
  * 函数功能: 串口接收完成回调函数
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明：无
  */
//void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle)
//{
// // HAL_UART_Transmit(&huartx,&aRxBuffer,1,0);
//   uat5C++; 
//  HAL_UART_Receive_IT(&STM32_USARTHandle,readServoPos,8);
//}



